using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;
using System.Diagnostics;

namespace Verlet
{
    class Quadruped : Constraint
    {
        LimbPair arms, legs;
        Node spineTop, spineBottom;
        Link spine;
        float legLength;
        float spineLength;
        float pelvisWidth;

        public bool showDebugLines = false;

        public float pGain = 100f;
        public float tGain = 1000f;
        public float standGain = 1000f;
        public float shoulderRestorationGain = 30f;
        public float pelvisRestorationGain = -2000f;
        public float faceForwardGain = 100f;
        public float faceWalkGain = 20f;

        public bool sitUp = true;
        public bool standUp = true;
        public bool walk = false;
        public bool swingArms = true;    // swing arms when walking
        public Vector3 walkVector = new Vector3(-2, 0, 0);
        public float stepTargetHeight = 0.1f;
        public float targetKneeBend = 0.05f;
        //int stepTimer = 0;

        // Updated in Update()
        // Midpoints of feet, pelvis, shoulders
        public Vector3 footMidpoint;
        public Vector3 pelvis;
        public Vector3 torso;
        // Shoulder-centered coordinate system
        public Vector3 bodyUp;
        public Vector3 bodyRight;
        public Vector3 bodyForward;
        // Pelvis-centered
        public Vector3 pelvisRight;
        public Vector3 pelvisForward;
        // Foot-centered
        public Vector3 footForward;

        // Walking parameters
        public float stride = 0.75f;  // Distance ahead of body foot is stepped
        public float ballisticStepTime = 0.5f;  // time spent moving foot forward
        bool leftLegNext;
        
        // Updated in Constrain()
        Vector3 leftLegDirection;
        Vector3 leftLegForward;
        Vector3 leftLegPlaneNormal;
        Vector3 leftKneeForward;
        Vector3 rightLegDirection;
        Vector3 rightLegForward;
        Vector3 rightLegPlaneNormal;
        Vector3 rightKneeForward;

        public Quadruped(float spineLength, float armSpacing, float armRadius, float upperArm, float foreArm,
            float pelvis, float legRadius, float femur, float shin)
        {
            this.spineLength = spineLength;
            pelvisWidth = pelvis;
            spineTop = new Node();
            spineBottom = new Node();
            spine = new Link(spineTop, spineBottom, spineLength, armRadius);
            arms = new LimbPair(spineTop, armSpacing, armRadius, upperArm, foreArm);
            legs = new LimbPair(spineBottom, pelvis, legRadius, femur, shin);
            //torso1 = new Link(arms.left.root, legs.left.root, torsoLength, legRadius);
            //torso2 = new Link(arms.right.root, legs.right.root, torsoLength, legRadius);
            legLength = femur + shin;

            // Normalize knee bend
            targetKneeBend *= spineLength;
            standGain /= spineLength;
        }

        public void Teleport(Vector3 position)
        {
            Vector3 offset = new Vector3(0, 0, pelvisWidth / 2);
            legs.left.end.Position = position - offset;
            legs.left.end.ControlMode = ControlMode.Fixed;
            legs.right.end.Position = position + offset;
            legs.right.end.ControlMode = ControlMode.Fixed;
            for (int i = 0; i < 100; i++)
            {
                Update();
                Constrain();
            }
            legs.left.end.ControlMode = ControlMode.Free;
            legs.right.end.ControlMode = ControlMode.Free;
        }

        public override void Constrain()
        {
            arms.Constrain();
            legs.Constrain();
            spine.Constrain();

#if constraintkneeseparation
            // kneeDist needs to be positive
            // i.e. the right knee needs to be to the right
            // of the left knee.
            const float minKneeDist = 0.2f;
            float kneeDist = Vector3.Dot(pelvisRight, legs.right.joint.Position - legs.left.joint.Position);
            if (kneeDist < minKneeDist)
            {
                Vector3 motion = pelvisRight * (minKneeDist - kneeDist) * 0.5f;
                legs.right.joint.Position += motion;
                legs.left.joint.Position -= motion;
            }
#endif

            // footDist needs to be positive
            // i.e. the right foot needs to be to the right
            // of the left foo.
            const float minFootDist = 0.2f;
            float footDist = Vector3.Dot(pelvisRight, legs.right.end.Position - legs.left.end.Position);
            if (footDist < minFootDist)
            {
                Vector3 motion = pelvisRight * (minFootDist - footDist) * 0.5f;
                legs.right.end.Position += motion;
                legs.left.end.Position -= motion;
            }

            // Constrain knees to lie in plane formed by hip-foot vector and "forward" direction
            leftLegDirection = legs.left.root.Position - legs.left.end.Position;
            leftLegForward = Vector3.Cross(leftLegDirection, pelvisRight);
            leftLegPlaneNormal = Vector3.Cross(leftLegForward, leftLegDirection);
            leftLegPlaneNormal.Normalize();
            // Distance knee is offset from the plane relative to foot
            float leftKneeError = Vector3.Dot(leftLegPlaneNormal, legs.left.joint.Position - legs.left.end.Position);
            Vector3 loffset = 0.5f * leftKneeError * leftLegPlaneNormal;
            legs.left.joint.Position -= loffset;
            legs.left.end.Position += loffset;
            leftKneeForward = Vector3.Cross(leftLegPlaneNormal, leftLegDirection);
            leftKneeForward.Normalize();
            float leftKneeBend = Vector3.Dot(leftKneeForward, legs.left.joint.Position - legs.left.end.Position);
            if (leftKneeBend < targetKneeBend)
            {
                Vector3 lkbCorrection = 0.5f * (leftKneeBend-targetKneeBend) * leftKneeForward;
                legs.left.joint.Position -= lkbCorrection;
                legs.left.end.Position += lkbCorrection;
            }


            rightLegDirection = legs.right.root.Position - legs.right.end.Position;
            rightLegForward = Vector3.Cross(pelvisRight, rightLegDirection);
            rightLegPlaneNormal = Vector3.Cross(rightLegForward, rightLegDirection);
            rightLegPlaneNormal.Normalize();
            // Distance knee is offset from the plane relative to foot
            float rightKneeError = Vector3.Dot(rightLegPlaneNormal, legs.right.joint.Position - legs.right.end.Position);
            Vector3 roffset = 0.5f * rightKneeError * rightLegPlaneNormal;
            legs.right.joint.Position -= roffset;
            legs.right.end.Position += roffset;

            // Note reversal of cross product relative to left leg.
            rightKneeForward = Vector3.Cross(rightLegDirection, rightLegPlaneNormal);
            rightKneeForward.Normalize();
            float rightKneeBend = Vector3.Dot(rightKneeForward, legs.right.joint.Position - legs.right.end.Position);
            if (rightKneeBend < targetKneeBend)
            {
                Vector3 rkbCorrection = 0.5f * (rightKneeBend-targetKneeBend) * rightKneeForward;
                legs.right.joint.Position -= rkbCorrection;
                legs.right.end.Position += rkbCorrection;
            }


            //torso1.Constrain();
            //torso2.Constrain();
        }

        public void Draw(Device d)
        {
            Link.PrepareDevice(d);
            if (Link.renderLines)
            {
                d.RenderState.Lighting = false;
                d.VertexFormat = CustomVertex.PositionColored.Format;
            }
            //torso1.Draw(d);
            //torso2.Draw(d);
            spine.Draw(d);
            arms.Draw(d);
            legs.Draw(d);
            if (Link.renderLines)
                d.RenderState.Lighting = true;

            if (showDebugLines)
            {
                d.RenderState.Lighting = false;
                Link.DrawLine(d, legs.left.root.Position, legs.left.root.Position + 3.0f * pelvisForward, new ColorValue(0.0f, 1.0f, 0.0f));
                Link.DrawLine(d, arms.left.root.Position, arms.left.root.Position + 3.0f * bodyForward, new ColorValue(0.0f, 1.0f, 0.0f));
                Link.DrawLine(d, legs.left.joint.Position, legs.left.joint.Position + leftKneeForward, new ColorValue(0.0f, 0.0f, 1.0f));
                Link.DrawLine(d, legs.right.joint.Position, legs.right.joint.Position + rightKneeForward, new ColorValue(0.0f, 0.0f, 1.0f));
                Link.DrawLine(d, torso, torso + 2.0f * walkVector, new ColorValue(1.0f, 1.0f, 0.0f));
                d.RenderState.Lighting = true;
            }
        }

        public void HangAt(Vector3 position)
        {
            arms.left.root.Position = position + new Vector3(-arms.spacing / 2.0f, 0, 0);
            arms.left.root.ControlMode = ControlMode.Fixed;
            arms.right.root.Position = position + new Vector3(arms.spacing / 2.0f, 0, 0);
            arms.right.root.ControlMode = ControlMode.Fixed;
        }

        public void UnHang()
        {
            arms.left.root.ControlMode = ControlMode.Free;
            arms.right.root.ControlMode = ControlMode.Free;
        }

        public void Update()
        {
            Vector3 gravity = new Vector3(0.0f, Node.Gravity, 0.0f);

            // Compute walking parameters
            Vector3 walkUnit = walkVector;
            float walkSpeed = walkVector.Length();
            if (walkSpeed>0.0f)
                walkUnit.Normalize();

            stride = Math.Min(0.5f*walkSpeed / legLength, legLength/2f);
            ballisticStepTime = 2f*stride/walkSpeed;

            footMidpoint = (legs.left.end.Position + legs.right.end.Position) * .5f;
            footMidpoint.Y = 0;  // Just in case a foot is lifted for a step
            pelvis = (legs.left.root.Position + legs.right.root.Position) * 0.5f;
            torso = (arms.left.root.Position + arms.right.root.Position) * 0.5f;

            // Set up an egocentric coordinate system
            bodyUp = torso - pelvis;
            bodyUp.Normalize();
            bodyRight = arms.right.root.Position-arms.left.root.Position;
            bodyRight.Normalize();
            bodyForward = Vector3.Cross(bodyRight, bodyUp);
            bodyForward.Normalize();
            pelvisRight = legs.right.root.Position - legs.left.root.Position;
            pelvisRight.Normalize();
            pelvisForward = Vector3.Cross(pelvisRight, bodyUp);
            pelvisForward.Normalize();
            footForward = Vector3.Cross(legs.right.end.Position - legs.left.end.Position, bodyUp);
            footForward.Normalize();

            // Compute force to stand pelvis up
            Vector3 pForce;
            if (standUp)
            {
                pForce = (footMidpoint - pelvis) * pGain;
                pForce.Y = (((walk?1.0f:1.0f)*legLength) - pelvis.Y) * standGain;
                //if (walk)
                //    pForce += 1.0f*pelvisForward;
            }
            else
                pForce = gravity;

            // Compute force to sit torso up
            Vector3 tForce;
            if (standUp || sitUp)
            {
                tForce = ((walk?(walkUnit*stride*0.1f):new Vector3(0,0,0))+pelvis - torso) * tGain;
                
                if (torso.Y < pelvis.Y+(0.5f*spineLength))
                    // need a boost to overcome gravity
                    tForce.Y = 500;
                else 
                    tForce.Y = 0;
            }
            else
                tForce = gravity;

            // Even out shoulders
            Vector3 shoulderOffset = shoulderRestorationGain * Vector3.Dot(bodyUp, arms.left.root.Position - arms.right.root.Position) * bodyUp;

            // Align shoulders with pelvis
            Vector3 faceForwardError = pelvisRight - bodyRight;
            faceForwardError -= Vector3.Dot(faceForwardError, bodyUp) * bodyUp;   // remove up component
            faceForwardError *= faceForwardGain;

            // Align pelvis with walkVector
            float pelvisError = Vector3.Dot(pelvisRight, walkVector);
            Vector3 faceWalkForce = faceWalkGain * pelvisError * pelvisForward;

            Vector3 pelvisOffset = pelvisRestorationGain * Vector3.Dot(bodyUp, legs.left.root.Position - legs.right.root.Position) * bodyUp;

            // Apply forces and update
            arms.left.root.Acceleration = (tForce-shoulderOffset)-faceForwardError;
            arms.right.root.Acceleration = tForce+shoulderOffset+faceForwardError;
            legs.left.root.Acceleration = pForce+faceWalkForce+pelvisOffset;
            legs.right.root.Acceleration = (pForce-faceWalkForce)-pelvisOffset;
            //legs.left.joint.Acceleration = 5.0f*pelvisForward + new Vector3(0, Node.Gravity, 0);
            //legs.right.joint.Acceleration = 5.0f*pelvisForward + new Vector3(0, Node.Gravity, 0);

            if (walk)
            {
                legs.left.root.GroundVelocity = walkVector;
                legs.right.root.GroundVelocity = walkVector;
                if (!legs.left.InBallisticMotion && !legs.right.InBallisticMotion)
                {
                    if (leftLegNext)
                    {
                        if (legs.left.Extension > 0.85f || Vector3.Dot(pelvis - legs.left.end.Position, walkVector) > stride)
                        {
                            Vector3 target = stride * walkUnit + legs.left.root.Position;
                            target.Y = stepTargetHeight;
                            legs.left.Goto(target, ballisticStepTime);
                            if (swingArms)
                                arms.right.end.Acceleration = 100f * walkSpeed * bodyForward;
                            leftLegNext = false;
                        }
                    }
                    else
                    {
                        if (legs.right.Extension > 0.85f || Vector3.Dot(pelvis - legs.right.end.Position, walkVector) > stride)
                        {
                            Vector3 target = stride * walkUnit + legs.right.root.Position;
                            target.Y = stepTargetHeight;
                            legs.right.Goto(target, ballisticStepTime);
                            if (swingArms)
                                arms.left.end.Acceleration = 100f * walkSpeed * bodyForward;
                            leftLegNext = true;
                        }
                    }
                } else
                {
                    arms.left.end.Acceleration = gravity;
                    arms.right.end.Acceleration = gravity;
                }
            }
            else
            {
                legs.left.root.GroundVelocity = gravity;
                legs.right.root.GroundVelocity = gravity;
                legs.left.end.ControlMode = ControlMode.Free;
                legs.right.end.ControlMode = ControlMode.Free;
                arms.left.end.Acceleration = gravity;
                arms.right.end.Acceleration = gravity;
            }

            arms.Update();
            legs.Update();
        }
    }
}
